#include "stm32f4xx.h"                  // Device header
#include "stm32f4xx_conf.h"
#include "IST8310.h"
#include "BMI088.h"
#include "MahonyAHRS.h"
#include "IMUTemperatureControl.h"

float AttitudeAlgorithms_q[4];//姿态解算四元数
float AttitudeAlgorithms_RadYaw,AttitudeAlgorithms_RadPitch,AttitudeAlgorithms_RadRoll;//弧度制角度
float AttitudeAlgorithms_DegYaw,AttitudeAlgorithms_DegPitch,AttitudeAlgorithms_DegRoll;//角度制角度

/*
 *函数简介:姿态解算初始化
 *参数说明:无
 *返回类型:无
 *备注:定时器定时1ms,更新四元数并解算角度
 */
void AttitudeAlgorithms_Init(void)
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11,ENABLE);//开启时钟
	
	TIM_InternalClockConfig(TIM11);//选择时基单元的时钟
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;//配置时基单元
	TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;//配置时钟分频为1分频
	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;//配置计数器模式为向上计数
	TIM_TimeBaseInitStructure.TIM_Period=500-1;//配置自动重装值ARR
	TIM_TimeBaseInitStructure.TIM_Prescaler=336-1;//配置分频值PSC,默认频率1000Hz
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter=0;//配置重复计数单元的置为0
	TIM_TimeBaseInit(TIM11,&TIM_TimeBaseInitStructure);//初始化TIM2
	
	TIM_ClearFlag(TIM11,TIM_FLAG_Update);//清除配置时基单元产生的中断标志位
	
	TIM_ITConfig(TIM11,TIM_IT_Update,ENABLE);//使能更新中断
	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//选择NVIC分组
	
	NVIC_InitTypeDef NVIC_InitStructure;//配置NVIC（配置参数）
	NVIC_InitStructure.NVIC_IRQChannel=TIM1_TRG_COM_TIM11_IRQn;//选择中断通道为TIM11
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;//使能中断通道
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1;//TIM2的抢占优先级
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=1;//TIM2的响应优先级
	NVIC_Init(&NVIC_InitStructure);//初始化NVIC
	
	
	IST8310_Init();//初始化IST8310
	BMI088_Init();//初始化BMI088
	IMUTemperatureControl_Init();//启动恒温控制
	AttitudeAlgorithms_q[0]=1.0f;//初始化四元数

	TIM_Cmd(TIM11,ENABLE);//启动定时器
}

/*
 *函数简介:TIM11定时器更新中断函数
 *参数说明:无
 *返回类型:无
 *备注:定时1ms更新四元数并且解算角度
 */
void TIM1_TRG_COM_TIM11_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM11,TIM_IT_Update)==SET)//检测TIM2更新
	{
		TIM_ClearITPendingBit(TIM11,TIM_IT_Update);//清除标志位
		
		if(BMI088_Temperature>20)//检测温度,防止温度过低
		{
			//MahonyAHRS_SixAxesUpdate(AttitudeAlgorithms_q,BMI088_Accel[0],BMI088_Accel[1],BMI088_Accel[2],BMI088_Gyro[0],BMI088_Gyro[1],BMI088_Gyro[2]);//更新四元数
			MahonyAHRS_NineAxesUpdate(AttitudeAlgorithms_q,BMI088_Accel[0],BMI088_Accel[1],BMI088_Accel[2],BMI088_Gyro[0],BMI088_Gyro[1],BMI088_Gyro[2],IST8310_MagneticField[0],IST8310_MagneticField[1],IST8310_MagneticField[2]);//更新四元数
			MahonyAHRS_GetAngle(AttitudeAlgorithms_q,&AttitudeAlgorithms_RadYaw,&AttitudeAlgorithms_RadPitch,&AttitudeAlgorithms_RadRoll);//解算角度
			
			AttitudeAlgorithms_DegYaw=AttitudeAlgorithms_RadYaw*57.295779513082320876798154814105f;//转换为角度制
			AttitudeAlgorithms_DegPitch=AttitudeAlgorithms_RadPitch*57.295779513082320876798154814105f;
			AttitudeAlgorithms_DegRoll=AttitudeAlgorithms_RadRoll*57.295779513082320876798154814105f;
//			AttitudeAlgorithms_DegYaw=AttitudeAlgorithms_RadYaw*180.0f/3.141592653589793238462643383279f;
//			AttitudeAlgorithms_DegPitch=AttitudeAlgorithms_RadPitch*180.0f/3.141592653589793238462643383279f;
//			AttitudeAlgorithms_DegRoll=AttitudeAlgorithms_RadRoll*180.0f/3.141592653589793238462643383279f;
		}
	}
}
